#include "mainwindow.h"
#include "./ui_mainwindow.h"
#include <QDebug>
#include <filesystem>
#include <fstream>
#include <chrono>
#include <iomanip>
#include <opencv2/opencv.hpp>
#include "configread.h"

cv::Mat QImageToMat(const QImage& qimage) {
    switch (qimage.format()) {
        // 处理32位带透明度的图像
        case QImage::Format_ARGB32:
        case QImage::Format_ARGB32_Premultiplied: {
            return cv::Mat(qimage.height(), qimage.width(), CV_8UC4, (void*)qimage.bits(), qimage.bytesPerLine());
        }
        // 处理24位RGB图像
        case QImage::Format_RGB32: {
            cv::Mat mat(qimage.height(), qimage.width(), CV_8UC4, (void*)qimage.bits(), qimage.bytesPerLine());
            cv::cvtColor(mat, mat, cv::COLOR_BGRA2BGR); // 移除alpha通道
            return mat;
        }
        // 处理24位RGB888图像
        case QImage::Format_RGB888: {
            cv::Mat mat(qimage.height(), qimage.width(), CV_8UC3, (void*)qimage.bits(), qimage.bytesPerLine());
            cv::cvtColor(mat, mat, cv::COLOR_RGB2BGR); // Qt是RGB，OpenCV需要BGR
            return mat;
        }
        // 处理8位灰度图像
        case QImage::Format_Grayscale8: {
            return cv::Mat(qimage.height(), qimage.width(), CV_8UC1, (void*)qimage.bits(), qimage.bytesPerLine());
        }
        default:
            qWarning() << "Unsupported QImage format:" << qimage.format();
            return cv::Mat();
    }
}


MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    m_cameraThread = new CameraThread(this);
    m_calibThread = new calibThread(this);
//    m_calibImages.reserve(100);

    timer = new QTimer(this);
    robot = std::make_unique<RobotInterface>();    // 创建机器人接口实例

    if (!robot->loadConfigure("./CS_UserManual_Robot_State_Message.txt")) {          // 加载配置文件
        std::cout << "Load Configure file fail. Check file path" << std::endl;
    }


    // config.ini
    configReadMessage();
    if(m_cameraModel == "realsensed435")
    {
        m_cameraThread->CameraInit(1);
    }else if(m_cameraModel == "orb336")
    {
        m_cameraThread->CameraInit(2);
    }

    // 槽函数
    connect(m_cameraThread, &CameraThread::frameReady, this, &MainWindow::handleFrame);
    connect(timer, &QTimer::timeout, this, &MainWindow::timerSlot);
    connect(m_calibThread, &calibThread::sendRMS, this, &MainWindow::handleRMS);
    connect(m_calibThread, &calibThread::sendMatrix, this, &MainWindow::handleMatrix);
    connect(m_calibThread, &calibThread::sendIntrinsic, this, &MainWindow::handleIntrinsic);
    connect(ui->pb_capture, &QPushButton::clicked, this, &MainWindow::onhandleCapture);// capture
    connect(ui->pb_calib, &QPushButton::clicked, this, &MainWindow::onhandleCalib);// calib

    captureNum = 0;

    // folder
    // 获取当前时间并格式化为字符串
    auto now = std::chrono::system_clock::now();
    std::time_t now_time = std::chrono::system_clock::to_time_t(now);
    std::tm *tm_now = std::localtime(&now_time);
    m_folderName << std::put_time(tm_now, "%Y%m%d_%H%M%S");

    // 创建文件夹
    std::filesystem::create_directory(m_folderName.str());
    m_posesPath = m_folderName.str() + "/poses.txt";
    m_posesFile.open(m_posesPath);
}

MainWindow::~MainWindow()
{
    delete ui;
    delete m_cameraThread;
    m_cameraThread=nullptr;
    delete m_calibThread;
    m_posesFile.close();
    timer->stop();
}


void MainWindow::on_pb_capture_clicked()
{
}

void MainWindow::onhandleCapture()
{

    QMutexLocker locker(&m_mutex);
    cv::Mat tempImage = QImageToMat(tempQImage);

    std::string imagePath = m_folderName.str() + "/image_" + std::to_string(captureNum) + ".png";
    cv::imwrite(imagePath, tempImage);
    if(m_posesFile.is_open())
    {
        for (size_t j = 0; j < m_tcp.size(); ++j) {
            m_posesFile << m_tcp[j];
            if (j < m_tcp.size() - 1) {
                m_posesFile << ","; // 用空格分隔
            }
        }
        m_posesFile << "\n"; // 换行
        m_posesFile.flush();  // 强制写入磁盘
    }else
    {
        std::cout << "m_posesFile not open" << std::endl;
    }

    ui->log->appendPlainText("捕获数据   "  + QTime::currentTime().toString("hh:mm:ss.zzz"));

    captureNum++;
    ui->pte_captureNum->clear();
    ui->pte_captureNum->appendPlainText(QString::number(captureNum));
}


void MainWindow::on_pb_calib_clicked()
{
}

void MainWindow::onhandleCalib()
{
    // testcalib
//    std::string testfolderName = "/home/jpx/Documents/ros2-amr/3rdParty/eliteRobot_cpp_sdk/HandEyeCalib/calib4.7";
//    std::string testposePath = testfolderName + "/poses.txt";
//    m_calibThread->myHandEyeCalib->CalibInit(testfolderName, testposePath);

    m_calibThread->myHandEyeCalib->CalibInit(m_folderName.str(), m_posesPath);
    m_calibThread->run();

    ui->log->appendPlainText("标定程序执行   " + QTime::currentTime().toString("hh:mm:ss.zzz"));

}

void MainWindow::on_pb_reset_clicked()
{
    this->close();
}


void MainWindow::on_pb_start_clicked()
{

    m_cameraThread->startCamera();

    QString ip = ui->eliteIP->text().trimmed();
    m_eliteIP = ip.toStdString();
    ui->log->appendPlainText("eliteIp = " + ip + "    " + QTime::currentTime().toString("hh:mm:ss.zzz"));

    if (!robot->connect(m_eliteIP, 30001)) {                                          // 连接到机器人
        std::cout << "Connect fail check network or IP" << std::endl;
    }

    // 启动定时器，每隔1000毫秒触发一次
    timer->start(1000);
}

// 槽函数
void MainWindow::handleFrame(const QImage &image)
{
    ui->label_camera->setPixmap(QPixmap::fromImage(image));

    QMutexLocker locker(&m_mutex);
    tempQImage = QImage();
    tempQImage = image.copy();

}

void MainWindow::timerSlot()
{
    QString logEntry;
    m_tcp = robot->getTcpPosition();
    // 添加所有TCP位置数据
    for (auto TcpPosition : m_tcp) {
        logEntry += QString::number(TcpPosition) + "   ";
    }
    // 输出到日志窗口
    ui->log->appendPlainText(logEntry + "  " + QTime::currentTime().toString("hh:mm:ss.zzz"));
}

void MainWindow::handleRMS(double rms)
{
    ui->pte_RE->clear();
    ui->pte_RE->appendPlainText(QString::number(rms));
}
void MainWindow::handleMatrix(std::vector<double> matrix)
{
    // 确保矩阵大小为 16
    if (matrix.size() != 16) {
        // 处理错误，例如返回或显示消息
        return;
    }

    QString matrixString;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            // 添加元素到字符串，格式化为 4x4
            matrixString += QString::number(matrix[i * 4 + j], 'f', 5) + "  "; // 保留两位小数
        }
        matrixString += "\n"; // 换行
    }

    // 设置 pte_CameraToEnd 的文本
    ui->pte_CameraToEnd->setPlainText(matrixString);
}

void MainWindow::handleIntrinsic(std::vector<double> intrinsic)
{
    if (intrinsic.size() != 4) {
        // 处理错误，例如返回或显示消息
        return;
    }

    QString QStringIntrinsic;
    for(int i = 0; i < 4; ++i)
    {
        QStringIntrinsic += QString::number(intrinsic[i], 'f', 5) + "  ";
    }
    ui->pte_CameraIntrinsic->setPlainText(QStringIntrinsic);

}

void MainWindow::configReadMessage()
{
    // read config.ini
    ConfigRead config;
    config.load("./config.ini");
    m_cameraModel = config.get("Camera", "CameraIndex");
    std::cout << "Value of Camera.CameraIndex: " << m_cameraModel << std::endl;
}
